#include "uni_motor.h"
#include "uni_servo.h"
#include "WonderK210.h"
#include "config.h"

#define BORD_RATE 115200//设定串口波特率为115200
UNI_MOTOR motor1(1, true);//定义编码电机1
UNI_MOTOR motor2(2, true);//定义编码电机2
UNI_MOTOR motor3(3, true);//定义编码电机3
UNI_MOTOR motor4(4, true);//定义编码电机4
WonderK210* wk;//接收控制器
Find_Box_Msg_st* result;//消息存储变量
UNI_SERVO_S servo(4, 6, 7, 10);//声明舵机4,6,7,10处的舵机
int stepper_speed = 4800; //设定步进电机速度

QR_MSG qr_msg;  //声明二维码需要的变量结构体
COLOR_MSG color_msg; //声明颜色识别需要的变量结构体
DETECT_MSG detect_msg; //将识别到的二维码、颜色信息转换为可识别的数据

void setup() {
  delay(500);//开机等待500毫秒
  Serial.begin(BORD_RATE);//打开串口
  vision_init(); //视觉模块初始化
  //detect_qr();   //进入二维码检测，直到检测到二维码后，程序才退出
  //color_detect();//进入颜色检测，直到检测到三个颜色后，程序才退出
}

void loop() {

}

//进入二维码检测，直到检测到二维码后，程序才退出
void detect_qr(){
  qrcode_detection_loop();//小车进入二维码检测循环，直到小车接收到二维码，才会退出该循环
  Serial.print("检测到的二维码信息为:");Serial.println(detect_msg.qr_complete_data);
  Serial.print("二维码加号之前的数据:");Serial.println(detect_msg.before_the_plus_data);
  Serial.print("二维码加号之后的数据:");Serial.println(detect_msg.after_the_plus_data);
  delay(1000);
}

//进入颜色检测，直到检测到三个颜色后，程序才退出
void color_detect(){
  color_detection_loop(); //第一次检测颜色
  Serial.print("第一次检测到的颜色为:");Serial.println(detect_msg.recv_color_one);
  delay(1000);
  color_detection_loop(); //第二次检测颜色
  Serial.print("第二次检测到的颜色为:");Serial.println(detect_msg.recv_color_two);
  delay(1000);
  color_detection_loop(); //第三次检测颜色
  Serial.print("第三次检测到的颜色为:");Serial.println(detect_msg.recv_color_three);
  delay(1000);
}
